#include <robot_chassis/CRobotImu.hpp>
#include "robot_chassis/CTurnOnRobot.hpp"

using namespace std::chrono_literals;

CRobotImu::CRobotImu(const std::string& nodeName)
    : Node(nodeName), turnonRobot_(CTurnOnRobot::getInstance())
{
    // 声明参数
    this->declare_parameter<std::string>("imu_frame_id", "imu_link");
    this->declare_parameter<std::string>("base_frame_id", "base_link");
    this->declare_parameter<bool>("publish_tf", true);
    this->declare_parameter<double>("transform_x", 0.0);
    this->declare_parameter<double>("transform_y", 0.0);
    this->declare_parameter<double>("transform_z", 0.0);
    this->declare_parameter<double>("transform_roll", 0.0);
    this->declare_parameter<double>("transform_pitch", 0.0);
    this->declare_parameter<double>("transform_yaw", 0.0);

    // 获取参数
    imuFrameId_  = this->get_parameter("imu_frame_id").as_string();
    baseFrameId_ = this->get_parameter("base_frame_id").as_string();
    publishTf_   = this->get_parameter("publish_tf").as_bool();
    transformX_  = this->get_parameter("transform_x").as_double();
    transformY_  = this->get_parameter("transform_y").as_double();
    transformZ_  = this->get_parameter("transform_z").as_double();
    transformRoll_  = this->get_parameter("transform_roll").as_double();
    transformPitch_ = this->get_parameter("transform_pitch").as_double();
    transformYaw_   = this->get_parameter("transform_yaw").as_double();

    // 创建发布者
    pub_ = this->create_publisher<sensor_msgs::msg::Imu>("chassis/imu", 10);

    // 创建TF广播器
    tfBroadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // 创建定时器，100Hz
    timer_ = this->create_wall_timer(10ms, std::bind(&CRobotImu::callback, this));

    RCLCPP_INFO(this->get_logger(), 
                "IMU节点已启动 - imu_frame: %s, base_frame: %s, publish_tf: %s",
                imuFrameId_.c_str(), baseFrameId_.c_str(), publishTf_ ? "true" : "false");
}

CRobotImu::~CRobotImu() {}

void CRobotImu::getImuData()
{
    turnonRobot_.getImuData(imuData_);

    sensorImuData_.header.stamp    = imuData_.timestamp_;
    sensorImuData_.header.frame_id = imuFrameId_;

    // 线加速度 (m/s^2)
    sensorImuData_.linear_acceleration.x = imuData_.linearAccelerationXData_;
    sensorImuData_.linear_acceleration.y = imuData_.linearAccelerationYData_;
    sensorImuData_.linear_acceleration.z = imuData_.linearAccelerationZData_;

    // 角速度 (rad/s)
    sensorImuData_.angular_velocity.x = imuData_.angularVelocityXData_;
    sensorImuData_.angular_velocity.y = imuData_.angularVelocityYData_;
    sensorImuData_.angular_velocity.z = imuData_.angularVelocityZData_;

    // 姿态未知，设置协方差第一个元素为 -1
    sensorImuData_.orientation_covariance[0] = -1;

    // 角速度协方差
    sensorImuData_.angular_velocity_covariance = {
        3.06e-7, 0,       0,
        0,       3.06e-7, 0,
        0,       0,       3.06e-7
    };

    // 线加速度协方差
    sensorImuData_.linear_acceleration_covariance = {
        8.64e-8, 0,       0,
        0,       8.64e-8, 0,
        0,       0,       8.64e-8
    };
}

void CRobotImu::publishTF()
{
    if (!publishTf_) {
        return;
    }

    geometry_msgs::msg::TransformStamped transformStamped;

    transformStamped.header.stamp    = sensorImuData_.header.stamp;
    transformStamped.header.frame_id = baseFrameId_;
    transformStamped.child_frame_id  = imuFrameId_;

    transformStamped.transform.translation.x = transformX_;
    transformStamped.transform.translation.y = transformY_;
    transformStamped.transform.translation.z = transformZ_;

    // 将欧拉角转换为四元数
    tf2::Quaternion q;
    q.setRPY(transformRoll_, transformPitch_, transformYaw_);
    transformStamped.transform.rotation.x = q.x();
    transformStamped.transform.rotation.y = q.y();
    transformStamped.transform.rotation.z = q.z();
    transformStamped.transform.rotation.w = q.w();

    tfBroadcaster_->sendTransform(transformStamped);
}

void CRobotImu::callback()
{
    getImuData();
    pub_->publish(sensorImuData_);
    publishTF();
}
